#include "SDF.h"
#include <iostream>
#include <qmath.h>
//#include <math.h>
SDF::SDF() : osg::Node()
{
    m_dof = new SDFDof;

    m_collisionEnable = true;
}

SDF::~SDF()
{

}

int SDF::getRigidPtrInt()
{
    return m_rigidPtrInt;
}

void SDF::setRigidPtrInt(int intPtr)
{
    m_rigidPtrInt = intPtr;
    m_dof->setSdfRigidPtrInt(intPtr);
}

int SDF::getRigidPtrIntActual()
{
    return m_rigidPtrIntActual;
}

void SDF::setRigidPtrIntActual(int intPtr)
{
    m_rigidPtrIntActual = intPtr;
}

std::string SDF::getOsgNodeName()
{
    return m_osgNodeName;
}

void SDF::setOsgNodeName(const std::string &nodeName)
{
    m_osgNodeName = nodeName;
}

osg::Vec3 SDF::getMinCoordinate()
{
    return m_minCoordinate;
}

void SDF::setMinCoordinate(double x, double y, double z)
{
    m_minCoordinate.set(x,y,z);
    //std::cout<<"SDF::setMinCoordinate x,y,z="<<x<<","<<y<<","<<z<<std::endl;
}

osg::Vec3 SDF::getNodeCountVector()
{
    return m_nodeCountVector;
}

void SDF::setNodeCountVector(int x, int y, int z)
{
    m_nodeCountVector.set(x,y,z);
   // std::cout<<"SDF::setNodeCountVector x,y,z="<<x<<","<<y<<","<<z<<std::endl;

}

int SDF::getNodeCount()
{
    return m_nodeCount;
}

void SDF::setNodeCount(int intValue)
{
    m_nodeCount = intValue;
    //std::cout<<"SDF::setNodeCount m_nodeCount="<<intValue<<std::endl;

}

osg::Vec3 SDF::getdxdydz()
{
    return m_dxdydz;
}

void SDF::setdxdydz(double dx, double dy, double dz)
{
    m_dxdydz.set(dx,dy,dz);
    //std::cout<<"SDF::setdxdydz dx,dy,dz="<<dx<<","<<dy<<","<<dz<<std::endl;

}

osg::Vec3 SDF::getMaxCoordinate()
{
    return m_maxCoordinate;
}

void SDF::setMaxCoordinate(double x, double y, double z)
{
    m_maxCoordinate.set(x,y,z);
    //std::cout<<"SDF::setMaxCoordinate x,y,z="<<x<<","<<y<<","<<z<<std::endl;

}

int SDF::getPointCount()
{
    return m_pointCount;
}

void SDF::setPointCount(int intValue)
{
    m_pointCount = intValue;
    //std::cout<<"SDF::setPointCount m_pointCount="<<intValue<<std::endl;

}

osg::Vec3 SDF::getPointCoordinate(int index)
{
    osg::Vec3 result;
    if(index >= 0 && index<m_samplePoint.size())
        result= m_samplePoint[index];
    return result;
}

osg::Vec3 SDF::getPointCoordinateInWorldFrame(int index)
{
    osg::Vec3 point = getPointCoordinate(index);
    osg::Matrix inverseMatrix = m_matrix;
    point = inverseMatrix.getTrans() + point;
    inverseMatrix.setTrans(0,0,0);
    point = inverseMatrix*point;
    return point;
}

void SDF::setMatrix(const osg::Matrix &m)
{
    m_matrix = m;
}

osg::Matrix SDF::getMatrix()
{
    return m_matrix;
}

std::vector<float> &SDF::getGridValue()
{
    return m_gridValue;
}

std::vector<osg::Vec3> &SDF::getSampleValue()
{
    return m_samplePoint;
}

bool SDF::isPointInGridInWorldFrame(float x, float y, float z)
{
    osg::Matrix inverseMatrix = osg::Matrix::inverse(m_matrix);

    osg::Vec3 point(x,y,z);
    point = inverseMatrix.getTrans() + point;
    inverseMatrix.setTrans(0,0,0);
    point = inverseMatrix*point;

    x = point.x();
    y = point.y();
    z = point.z();

    //std::cout<<"SDF::isPointInGridInWordFrame pointCor="<<x<<","<<y<<","<<z<<std::endl;
    //std::cout<<"SDF::isPointInGridInWordFrame pointCor="<<m_matrix.getTrans().x()<<","<<m_matrix.getTrans().y()<<","<<m_matrix.getTrans().z()<<std::endl;

    bool isX = x >= m_minCoordinate.x() && x <= m_maxCoordinate.x();
    bool isY = y >= m_minCoordinate.y() && y <= m_maxCoordinate.y();
    bool isZ = z >= m_minCoordinate.z() && z <= m_maxCoordinate.z();

    bool result = false;
    if(isX && isY && isZ)
        result = true;
    return result;
}

bool SDF::isPointClosedInWorldFrame(float x, float y, float z)
{
    float dx = m_dxdydz.x();float dy = m_dxdydz.y();float dz = m_dxdydz.z();
    float gridValue = qAbs(getGridValueByWorldFrame(x,y,z));
    float max_d = qMax(dx, qMax(dy, dz));
    bool result = false;
    if(gridValue < max_d)
    {
        if(isPointEncloseClosedInWorldFrame(x,y,z))
        {
            result = true;
        }
    }

    return result;
}

bool SDF::isPointClosedByIJK(int x, int y, int z)
{
    float dx = m_dxdydz.x();float dy = m_dxdydz.y();float dz = m_dxdydz.z();
    float gridValue = getGridValueByIJK(x,y,z);
    osg::Vec3 gradient = getGradientByIJK(x,y,z);
    gradient.normalize();
    int i,j,k;
    if(gradient.x() > 0)
    {
        i = x-1;
    }else if(gradient.x() < 0)
    {
        i = x + 1;
    }else
    {
        i = x;
    }

    if(gradient.y() > 0)
    {
        j = y-1;
    }else if(gradient.y() < 0)
    {
        j = y + 1;
    }else
    {
        j = y;
    }

    if(gradient.z() > 0)
    {
        k = z-1;
    }else if(gradient.z() < 0)
    {
        k = z + 1;
    }else
    {
        k = z;
    }
    float gridValueForCharge = getGridValueByIJK(i,j,k);

    bool result = false;
    if(gridValueForCharge * gridValue < 0)
    {
        result = true;
    }
    return result;
}

bool SDF::isPointClosedByIJK2(int x, int y, int z)
{
    float dx = m_dxdydz.x();float dy = m_dxdydz.y();float dz = m_dxdydz.z();
    double value = getGridValueByIJK(x,y,z);
    float max_d = (dx + dy + dz)/3.0;//qMax(dx, qMax(dy, dz));
    bool result = true;
    if(qAbs(value) < 3*max_d)
    {
        result = true;
    }
    else
    {
        //std::cout<<"SDF::isPointClosedByIJK2 IJK="<<x<<","<<y<<","<<z<<std::endl;
        //std::cout<<"SDF::isPointClosedByIJK2 value="<<value<<std::endl;
        result = false;
    }
    return result;
}

bool SDF::isPointClosedByIJK3(int x, int y, int z)
{
    float dx = m_dxdydz.x();float dy = m_dxdydz.y();float dz = m_dxdydz.z();
    double value = getGridValueByIJK(x,y,z);
    float max_d = qMax(dx, qMax(dy, dz));
    float min_d = qMin(dx, qMin(dy, dz));


    if(value > max_d && qAbs(value - max_d) > 10e-6 || value < 0 || qAbs(value) < 10e-5)
    {
        return false;
    }

    bool result = false;
    std::vector<float> values;
    values.push_back(getGridValueByIJK(x+1,y,z));
    values.push_back(getGridValueByIJK(x-1,y,z));
    values.push_back(getGridValueByIJK(x,y+1,z));
    values.push_back(getGridValueByIJK(x,y-1,z));
    values.push_back(getGridValueByIJK(x,y,z+1));
    values.push_back(getGridValueByIJK(x,y,z-1));
    std::vector<bool> boolValue;
    for(int i= 0; i < values.size(); i++)
    {
        if(qAbs(values.at(i)-0) < 10e-6 )
        {
            boolValue.push_back(false);
        }
        else if(values.at(i) < 0)
        {
            boolValue.push_back(false);
        }
        else
        {
            boolValue.push_back(true);
        }
    }
    int falseCount = 0;
    for(int i = 0; i< boolValue.size(); i++)
    {
        if(boolValue[i] == false)
        {
            falseCount++;
        }
    }
    if(falseCount >0 && falseCount <6)
    {
        result = true;
    }
    else
    {
        result = false;
    }
    //std::cout<<"SDF::isPointClosedByIJK3 falseCount"<<falseCount<<std::endl;
    return result;
}

bool SDF::isPointClosedByIJK4(int x, int y, int z)
{
    bool result = false;
    if(isPointClosedByIJK3(x,y,z) == true)
    {
        double value = getGridValueByIJK(x,y,z);

        std::vector<float> values;
        std::vector<bool> boolValues;

        values.push_back(getGridValueByIJK(x+1,y,z));
        values.push_back(getGridValueByIJK(x-1,y,z));
        values.push_back(getGridValueByIJK(x,y+1,z));
        values.push_back(getGridValueByIJK(x,y-1,z));
        values.push_back(getGridValueByIJK(x,y,z+1));
        values.push_back(getGridValueByIJK(x,y,z-1));

        boolValues.push_back(isPointClosedByIJK3(x+1,y,z));
        boolValues.push_back(isPointClosedByIJK3(x-1,y,z));
        boolValues.push_back(isPointClosedByIJK3(x,y+1,z));
        boolValues.push_back(isPointClosedByIJK3(x,y-1,z));
        boolValues.push_back(isPointClosedByIJK3(x,y,z+1));
        boolValues.push_back(isPointClosedByIJK3(x,y,z-1));
        for(int i = 0; i < values.size(); i++)
        {
            if( boolValues[i] == true && values[i] < value && qAbs(value - values[i]) > 10e-6)//
            {
                result = false;
                break;
            }
            else
            {
                result = true;
            }
        }

    }
    return result;
}

bool SDF::isPointEncloseClosedInWorldFrame(float x, float y, float z)
{
    int i0,j0,k0,i1,j1,k1;
    enclosing_indices(getPositionInLocalFrame(x,y,z),i0,j0,k0,i1,j1,k1);
    bool result = true;
    result &= isPointClosedByIJK2(i0,j0,k0);
    result &= isPointClosedByIJK2(i0,j0,k1);
    result &= isPointClosedByIJK2(i0,j1,k0);
    result &= isPointClosedByIJK2(i0,j1,k1);
    result &= isPointClosedByIJK2(i1,j0,k0);
    result &= isPointClosedByIJK2(i1,j0,k1);
    result &= isPointClosedByIJK2(i1,j1,k0);
    result &= isPointClosedByIJK2(i1,j1,k1);

    if(result == false)
    {
        //std::cout<<"SDF::isPointEncloseClosed point="<<x<<","<<y<<","<<z<<std::endl;
    }
    return result;
}


bool SDF::isPointNearSharpEdgeInWorldFrame(float x, float y, float z)
{
    bool result = false;
    osg::Vec3 pointLocal = getPositionInLocalFrame(x,y,z);
    result = isPointNearSharpEdge(pointLocal.x(),pointLocal.y(),pointLocal.z());
    return result;

}

bool SDF::isPointNearSharpEdge(float x, float y, float z)
{
    int countX = m_nodeCountVector.x();
    int countY = m_nodeCountVector.y();
    int countZ = m_nodeCountVector.z();

    int i0,i1,j0,j1,k0,k1;
    enclosing_indices(osg::Vec3(x,y,z),i0, j0, k0, i1, j1, k1);
    bool isNormalParallel = isNormalParallelBy2Index(i0,j0,k0,i1,j1,k1);

    bool result = false;
    i0 = qMax(i0-1, 0);i1 = qMin(i1+1, countX-1);
    j0 = qMax(j0-1, 0);j1 = qMin(j1+1, countY-1);
    k0 = qMax(k0-1, 0);k1 = qMin(k1+1, countZ-1);

    //i0,j0,z0,  i0,j0,z1, i0,j1,z0, i0,j1,z1
    {
        float v0 = getGridValueByIJK(i0,j0,k0);
        float v1 = getGridValueByIJK(i0,j0,k1);
        float v2 = getGridValueByIJK(i0,j1,k0);
        float v3 = getGridValueByIJK(i0,j1,k1);
        result |= isEdgeThroughBy4Value(v0,v1,v2,v3);
    }
    {
        float v0 = getGridValueByIJK(i1,j0,k0);
        float v1 = getGridValueByIJK(i1,j0,k1);
        float v2 = getGridValueByIJK(i1,j1,k0);
        float v3 = getGridValueByIJK(i1,j1,k1);
        result |= isEdgeThroughBy4Value(v0,v1,v2,v3);
    }
    {
        float v0 = getGridValueByIJK(i0,j0,k0);
        float v1 = getGridValueByIJK(i0,j0,k1);
        float v2 = getGridValueByIJK(i1,j0,k0);
        float v3 = getGridValueByIJK(i1,j0,k1);
        result |= isEdgeThroughBy4Value(v0,v1,v2,v3);
    }
    {
        float v0 = getGridValueByIJK(i0,j1,k0);
        float v1 = getGridValueByIJK(i0,j1,k1);
        float v2 = getGridValueByIJK(i1,j1,k0);
        float v3 = getGridValueByIJK(i1,j1,k1);
        result |= isEdgeThroughBy4Value(v0,v1,v2,v3);
    }
    {
        float v0 = getGridValueByIJK(i0,j0,k0);
        float v1 = getGridValueByIJK(i1,j0,k0);
        float v2 = getGridValueByIJK(i0,j1,k0);
        float v3 = getGridValueByIJK(i1,j1,k0);
        result |= isEdgeThroughBy4Value(v0,v1,v2,v3);
    }
    {
        float v0 = getGridValueByIJK(i0,j0,k1);
        float v1 = getGridValueByIJK(i1,j0,k1);
        float v2 = getGridValueByIJK(i0,j1,k1);
        float v3 = getGridValueByIJK(i1,j1,k1);
        result |= isEdgeThroughBy4Value(v0,v1,v2,v3);
    }
    if(result == true)
        result = !isNormalParallel;
    return result;
}

float SDF::getGridValueByIJK(int i, int j, int k)
{

    int m_I = (int)m_nodeCountVector.x();
    int m_J = (int)m_nodeCountVector.y();
    int m_K = (int)m_nodeCountVector.z();

    int ii = (i%m_I);
    if(ii<0)
      ii += m_I;
    int jj = (j%m_J);
    if(jj<0)
      jj += m_J;
    int kk = (k%m_K);
    if(kk<0)
      kk += m_K;
    long idx = (kk*m_J + jj)*m_I + ii;

    //std::cout<<"SDF::gridValueByIJK IJK="<<i<<","<<j<<","<<k<<std::endl;
    //std::cout<<"SDF::gridValueByIJK value="<<m_gridValue[idx]<<std::endl;
    return m_gridValue[idx];

}

float SDF::getGridValueByPosition(float x, float y, float z)
{
    float dx = m_dxdydz.x();
    float dy = m_dxdydz.y();
    float dz = m_dxdydz.z();

    int I = m_nodeCountVector.x();
    int J = m_nodeCountVector.y();
    int K = m_nodeCountVector.z();

    float minX = m_minCoordinate.x();
    float minY = m_minCoordinate.y();
    float minZ = m_minCoordinate.z();

    int i0,i1,j0,j1,k0,k1;
    enclosing_indices(osg::Vec3(x,y,z),i0, j0, k0, i1, j1, k1);

    //x = position.x();y = position.y(); z = position.z();

    float d000 = getGridValueByIJK( i0, j0, k0 );
    float d001 = getGridValueByIJK( i1, j0, k0 );
    float d010 = getGridValueByIJK( i0, j1, k0 );
    float d011 = getGridValueByIJK( i1, j1, k0 );
    float d100 = getGridValueByIJK( i0, j0, k1 );
    float d101 = getGridValueByIJK( i1, j0, k1 );
    float d110 = getGridValueByIJK( i0, j1, k1 );
    float d111 = getGridValueByIJK( i1, j1, k1 );

    float s = ( x - ( i0*dx + minX ) ) / dx;
    float t = ( y - ( j0*dy + minY ) ) / dy;
    float u = ( z - ( k0*dz + minZ ) ) / dz;

    float x00 =  (d001 - d000)*s + d000;
    float x01 =  (d011 - d010)*s + d010;
    float x10 =  (d101 - d100)*s + d100;
    float x11 =  (d111 - d110)*s + d110;
    float y0  =  ( x01 -  x00)*t +  x00;
    float y1  =  ( x11 -  x10)*t +  x10;

    float value;
    value =  (y1-y0)*u + y0;

    return value;

}

float SDF::getGridValueByWorldFrame(float x, float y, float z)
{
    osg::Vec3 position = getPositionInLocalFrame(x,y,z);

    float value;
    value = getGridValueByPosition(position.x(), position.y(), position.z());

    return value;
}

osg::Vec3 SDF::getPositionInSDFFrameByIJK(int i, int j, int k)
{
    int m_I = (int)m_nodeCountVector.x();
    int m_J = (int)m_nodeCountVector.y();
    int m_K = (int)m_nodeCountVector.z();

    float dx = m_dxdydz.x();
    float dy = m_dxdydz.y();
    float dz = m_dxdydz.z();

    float minX = m_minCoordinate.x();
    float minY = m_minCoordinate.y();
    float minZ = m_minCoordinate.z();

    osg::Vec3 pos;
    if(i >= 0 && i < m_I && j >= 0 && j < m_J && k >= 0 && k < m_K)
    {
        float posX = i*dx + minX;
        float posY = j*dy + minY;
        float posZ = k*dz + minZ;
        pos.set(posX, posY, posZ);
    }
    else
    {
        pos = osg::Vec3(0,0,-30);
    }
    return pos;
}

osg::Vec3 SDF::getGradientByIJK(int i, int j, int k)
{
    osg::Vec3 gradient;
    int I = m_nodeCountVector.x();
    int J = m_nodeCountVector.y();
    int K = m_nodeCountVector.z();

    float dx = m_dxdydz.x();
    float dy = m_dxdydz.y();
    float dz = m_dxdydz.z();

    int im1 = 0, jm1 = 0, km1 = 0;
    if ( i>0 )
      im1 = i - 1;
    if ( j>0 )
      jm1 = j - 1;
    if ( k>0 )
      km1 = k - 1;

    int ip1 = qMin( i + 1, I - 1 );
    int jp1 = qMin( j + 1, J - 1 );
    int kp1 = qMin( k + 1, K - 1 );

    int idx     = ( k   * J + j )   * I + i;
    int idx_im1 = ( k   * J + j )   * I + im1;
    int idx_ip1 = ( k   * J + j )   * I + ip1;
    int idx_jm1 = ( k   * J + jm1 ) * I + i;
    int idx_jp1 = ( k   * J + jp1 ) * I + i;
    int idx_km1 = ( km1 * J + j )   * I + i;
    int idx_kp1 = ( kp1 * J + j )   * I + i;

    float r_idx = m_gridValue[ idx ];
    float r_im1 = m_gridValue[ idx_im1 ];
    float r_ip1 = m_gridValue[ idx_ip1 ];
    float r_jm1 = m_gridValue[ idx_jm1 ];
    float r_jp1 = m_gridValue[ idx_jp1 ];
    float r_km1 = m_gridValue[ idx_km1 ];
    float r_kp1 = m_gridValue[ idx_kp1 ];

    float gradientX, gradientY,gradientZ;
    if ( i == 0 )
    {
      gradientX = ( r_ip1 - r_idx );
      gradientX /= dx;
    }
    else if ( i == ( I - 1 ) )
    {
      gradientX = ( r_idx - r_im1 );
      gradientX /= dx;
    }
    else
    {
      gradientX = ( r_ip1 - r_im1 );
      gradientX /= ( 2 * dx );
    }
    if ( j == 0 )
    {
      gradientY = ( r_jp1 - r_idx );
      gradientY /= dy;
    }
    else if ( j == ( J - 1 ) )
    {
      gradientY = ( r_idx - r_jm1 );
      gradientY /= dy;
    }
    else
    {
      gradientY = ( r_jp1 - r_jm1 );
      gradientY /= ( 2 * dy );
    }

    if ( k == 0 )
    {
      gradientZ = ( r_kp1 - r_idx );
      gradientZ /= dz;
    }
    else if ( k == ( K - 1 ) )
    {
      gradientZ = ( r_idx - r_km1 );
      gradientZ /= dz;
    }
    else
    {
      gradientZ = ( r_kp1 - r_km1 );
      gradientZ /= ( 2 * dz );
    }
    gradient.set(gradientX, gradientY, gradientZ);
    return gradient;
}

osg::Vec3 SDF::getGradientByPosition(float x, float y, float z)
{
    float dx = m_dxdydz.x();
    float dy = m_dxdydz.y();
    float dz = m_dxdydz.z();

    osg::Vec3 point(x,y,z);
    int i0,i1,j0,j1,k0,k1;
    enclosing_indices(point,i0, j0, k0, i1, j1, k1);

    osg::Vec3 g000,g001,g010,g011,g100,g101,g110,g111;
    g000 = getGradientByIJK( i0, j0, k0 );
    g001 = getGradientByIJK( i1, j0, k0 );
    g010 = getGradientByIJK( i0, j1, k0 );
    g011 = getGradientByIJK( i1, j1, k0 );
    g100 = getGradientByIJK( i0, j0, k1 );
    g101 = getGradientByIJK( i1, j0, k1 );
    g110 = getGradientByIJK( i0, j1, k1 );
    g111 = getGradientByIJK( i1, j1, k1 );

    float s = ( x - ( i0*dx + m_minCoordinate.x() ) ) / dx;
    float t = ( y - ( j0*dy + m_minCoordinate.y() ) ) / dy;
    float u = ( z - ( k0*dz + m_minCoordinate.z() ) ) / dz;

    osg::Vec3 x00 =  (g001 - g000)*s + g000;
    osg::Vec3 x01 =  (g011 - g010)*s + g010;
    osg::Vec3 x10 =  (g101 - g100)*s + g100;
    osg::Vec3 x11 =  (g111 - g110)*s + g110;
    osg::Vec3 y0  =  ( x01 -  x00)*t +  x00;
    osg::Vec3 y1  =  ( x11 -  x10)*t +  x10;

    osg::Vec3 gradient;
    gradient =  (y1-y0)*u + y0;
    return gradient;

}

osg::Vec3 SDF::getGradientByPositionInWorldFrame(float x, float y, float z)
{
    osg::Matrix inverseMatrix = osg::Matrix::inverse(m_matrix);

    osg::Vec3 point(x,y,z);
    point = inverseMatrix.getTrans() + point;
    inverseMatrix.setTrans(0,0,0);
    point = inverseMatrix*point;

    x = point.x();
    y = point.y();
    z = point.z();

    return getGradientByPosition(x,y,z);
}

void SDF::enclosing_indices(osg::Vec3 &point, int &i0, int &j0, int &k0, int &i1, int &j1, int &k1)
{
    using std::max;
    using std::min;
    using std::floor;

    float dx = m_dxdydz.x();
    float dy = m_dxdydz.y();
    float dz = m_dxdydz.z();

    int I = m_nodeCountVector.x();
    int J = m_nodeCountVector.y();
    int K = m_nodeCountVector.z();

    osg::Vec3 range = m_maxCoordinate - m_minCoordinate;
    osg::Vec3 diff  = point - m_minCoordinate;

    float diff_x = diff.x();float diff_y = diff.y();float diff_z = diff.z();

    //std::cout<<"SDF::enclosing_indices diff="<<diff_x<<","<<diff_y<<","<<diff_z<<std::endl;

    diff_x = qMax((float)0.0, diff_x);    diff_x = qMin(diff_x, (float)range.x());
    diff_y = qMax((float)0.0, diff_y);    diff_y = qMin(diff_y, (float)range.y());
    diff_z = qMax((float)0.0, diff_z);    diff_z = qMin(diff_z, (float)range.z());
    //diff.set(diff_x, diff_y, diff_z);

    diff.set(diff_x, diff_y, diff_z);
    point = diff + m_minCoordinate;

    i0 = std::floor( diff_x / dx);
    j0 = std::floor( diff_y / dy);
    k0 = std::floor( diff_z / dz);

    i1 = qMin(( i0 + 1 ) , I-1);
    j1 = qMin(( j0 + 1 ) , J-1);
    k1 = qMin(( k0 + 1 ) , K-1);
}

double SDF::getBoxSize()
{
    osg::Vec3 diagonal = m_maxCoordinate - m_minCoordinate;
    return diagonal.length();
}

osg::ref_ptr<SDFDof> SDF::getSDFDof()
{
    return m_dof;
}

void SDF::setCollisionEnable(bool boolValue)
{
    m_collisionEnable = boolValue;
}

bool SDF::getCollisionEnable()
{
    return m_collisionEnable;
}

osg::Vec3 SDF::getPositionInLocalFrame(float x, float y, float z)
{
    osg::Matrix inverseMatrix = osg::Matrix::inverse(m_matrix);

    osg::Vec3 point(x,y,z);
    point = inverseMatrix.getTrans() + point;
    inverseMatrix.setTrans(0,0,0);
    point = inverseMatrix*point;

    x = point.x();
    y = point.y();
    z = point.z();

    return osg::Vec3(x,y,z);
}

bool SDF::isEdgeThroughBy4Value(float v0, float v1, float v2, float v3)
{
    bool result = false;
    int countPo = 0;
    int countNe = 0;

    if(v0 > 0)
        countPo ++;
    else
        countNe ++;

    if(v1 > 0)
        countPo ++;
    else
        countNe ++;

    if(v2 > 0)
        countPo ++;
    else
        countNe ++;

    if(v3 > 0)
        countPo ++;
    else
        countNe ++;

    if(countPo + countNe == 4 )
    {
        if(countPo  == 3*countNe)
            result = true;
        else if(countNe  == 3*countPo)
            result = true;
    }
    return result;
}

bool SDF::isNormalParallelBy2Index(int i0, int j0, int k0, int i1, int j1, int k1)
{
    osg::Vec3 gradient0 = getGradientByIJK(i0,j0,k0);
    osg::Vec3 gradient1 = getGradientByIJK(i0,j0,k1);
    osg::Vec3 gradient2 = getGradientByIJK(i0,j1,k0);
    osg::Vec3 gradient3 = getGradientByIJK(i0,j1,k1);
    osg::Vec3 gradient4 = getGradientByIJK(i1,j0,k0);
    osg::Vec3 gradient5 = getGradientByIJK(i1,j0,k1);
    osg::Vec3 gradient6 = getGradientByIJK(i1,j1,k0);
    osg::Vec3 gradient7 = getGradientByIJK(i1,j1,k1);
    gradient0.normalize();gradient1.normalize();gradient2.normalize();
    gradient3.normalize();gradient4.normalize();gradient5.normalize();
    gradient6.normalize();gradient7.normalize();
    bool result = false;
    if(gradient0 == gradient7)
        result = true;
    return result;
}
